#include <ros/ros.h>
#include <move_base_msgs/MoveBaseAction.h>
#include <actionlib/client/simple_action_client.h>
#include "rotation_navigation/RotationAngle.h"
#include <tf/transform_listener.h>
#include <tf/transform_datatypes.h>

typedef actionlib::SimpleActionClient<move_base_msgs::MoveBaseAction> MoveBaseClient;

class RotationWorker {
public:
    RotationWorker() : ac_("move_base", true) {
        angle_sub_ = nh_.subscribe("/rotation_angle", 1, &RotationWorker::angleCallback, this);
        ac_.waitForServer();
    }

    void angleCallback(const rotation_navigation::RotationAngle::ConstPtr& msg) {
        double target_yaw;
        target_yaw = msg->angle;
        executeRotation(target_yaw);
    }

    void executeRotation(double target_yaw) {
        move_base_msgs::MoveBaseGoal goal;
        
        // 获取当前位置
        tf::StampedTransform transform;
        try {
            listener_.lookupTransform("map", "base_link", ros::Time(0), transform);
            
            goal.target_pose.pose.position.x = transform.getOrigin().x();
            goal.target_pose.pose.position.y = transform.getOrigin().y();
            
            tf2::Quaternion q;
            q.setRPY(0, 0, target_yaw);
            goal.target_pose.pose.orientation.x = q.x();
            goal.target_pose.pose.orientation.y = q.y();
            goal.target_pose.pose.orientation.z = q.z();
            goal.target_pose.pose.orientation.w = q.w();
            
            goal.target_pose.header.frame_id = "map";
            goal.target_pose.header.stamp = ros::Time::now();
            
            ac_.sendGoal(goal);
            ac_.waitForResult();
            
            if(ac_.getState() == actionlib::SimpleClientGoalState::SUCCEEDED) {
                ROS_INFO("旋转完成!");
            } else {
                ROS_WARN("旋转失败");
            }
        } catch (tf::TransformException &ex) {
            ROS_ERROR("TF错误: %s", ex.what());
        }
    }

private:
    ros::NodeHandle nh_;
    ros::Subscriber angle_sub_;
    MoveBaseClient ac_;
    tf::TransformListener listener_;
};

int main(int argc, char** argv) {
    ros::init(argc, argv, "rotation_worker");
    RotationWorker worker;
    ros::spin();
    return 0;
}